/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.command.CommandGroup;
import edu.wpi.first.wpilibj.command.WaitCommand;
import edu.wpi.first.wpilibj.templates.subsystems.ArmSubsystem;
import edu.wpi.first.wpilibj.templates.subsystems.ShooterSubsystem;

/**
 *
 * @author Jarrod Risley
 * 
 * NOTE: InchForward uses a JOYSTICK VALUE not a VOLTAGE
 * ALWAYS ADD PARALLEL BEFORE TO THE COMMAND BEFORE THE WAITCOMMAND!!!!!!
 * FUNCTION WILL RUN SINE FINE!!!!!!
 */
public class AutonMode extends CommandGroup {

    public AutonMode() {
        // Add Commands here:
        // e.g. addSequential(new Command1());
        //      addSequential(new Command2());
        // these will run in order.

        // To run multiple commands at the same time,
        // use addParallel()
        // e.g. addParallel(new Command1());
        //      addSequential(new Command2());
        // Command1 and Command2 will run in parallel.
        
        // A command group will require all of the subsystems that each member
        // would require.
        // e.g. if Command1 requires chassis, and Command2 requires arm,
        // a CommandGroup containing them would require both the chassis and the
        // arm.
//        for (int i = 0; i < 5; i++) {
//            addSequential(new Drive(1));          // Meters
//            addSequential(new Turn(180));         // Degrees
//            addSequential(new Drive(-1));         // Meters
//            addSequential(new Turn(-180));        // Degrees
//            addSequential(new Swerve(Swerve.SwerveState.kSwerveDeployed));
//            addSequential(new WaitCommand(2.2));  // Seconds
//            addSequential(new Swerve(Swerve.SwerveState.kSwerveRetracted));
//            addSequential(new WaitCommand(2.2));  // Seconds
//        }
        
        //HighClearance engaged before driving.
        System.out.println("Beginning Autonomous routine.");
        System.out.println("Setting High Clearance");
        addSequential(new HighClearance(true));

        //Raise Arm
        addParallel(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kRaised));
        addSequential(new WaitCommand(1.0));

        //Drive Forward
        addParallel(new InchForward(0.4));//This is a joystick value not a voltage!!!!!!!!!
        addSequential(new WaitCommand(3));
                
        // Inch forward so we know we are ramming the basket.
        addParallel(new InchForward(0.2));                          
        addSequential(new WaitCommand(1));
        
        //Shoot
        addParallel(new Shooter(ShooterSubsystem.ShooterValue.kShoot.value)); 
        addSequential(new WaitCommand(3)); 

        //Head Bob
        addParallel(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kIntermediate));
        addSequential(new WaitCommand(0.3));
        
        //End of Head Bob
        addParallel(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kRaised));
        addSequential(new WaitCommand(0.3));
        System.out.println("Shooting");
        //Shoot 
        addParallel(new Shooter(ShooterSubsystem.ShooterValue.kShoot.value)); 
        addSequential(new WaitCommand(3));
        
        //Stop Shooter, move Back
        addParallel(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kIntermediate));
        addSequential(new WaitCommand(0.3));
        addParallel(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kRaised));
        addSequential(new WaitCommand(0.3));
        addParallel(new Shooter(ShooterSubsystem.ShooterValue.kShoot.value)); 
        addSequential(new WaitCommand(4));
        addSequential(new Shooter(ShooterSubsystem.ShooterValue.kStop.value));
        addParallel(new InchForward(-0.2)); 
     	addSequential(new WaitCommand(1));
        
        //Drive Mode Fore, Lower Arm
        addSequential(new newDriveDirection(newDriveDirection.Direction.kFore));
        System.out.println("Lowering Arm");
        addParallel(new SetArmPosition(ArmSubsystem.ArmPresetLocation.kIntermediate));
        addSequential(new WaitCommand(1));
        System.out.println("Autonomous finished. Begining teleop mode.");
    
    }
}
